

#include "Constants.h"
#include "Gamepad.h"
#include "GearBox.h"








/*
 * Default constructor.
 */
GearBox::GearBox()
{

}



GearBox::GearBox(int port, int slot,  bool isRight)
{
	m_Jaguar1 = new Jaguar(port,slot); //pwm
	m_Jaguar2 = new Jaguar(port, slot+1);
	//m_Joystick = joystick;
	m_isRight = isRight;
	mptr_Encoder = new Encoder(slot,slot+1); //digital
	mptr_Encoder->SetDistancePerPulse(0.0243434865958677);  
	if (true)
	{
		mptr_Encoder->SetReverseDirection(true);
	}
	mptr_Encoder->Reset();
	mptr_Encoder->Start();

}


GearBox::~GearBox()
{

}



void GearBox::drive(float speedToJaguar){
	if (m_isRight)
		{
			m_Jaguar1 -> Set(speedToJaguar);
			m_Jaguar2 -> Set(speedToJaguar);
		}
	else
		{
			m_Jaguar1 -> Set(-speedToJaguar);
			m_Jaguar2 -> Set(-speedToJaguar);
		}

}

double GearBox::getEncoderValue(){
	return mptr_Encoder->GetDistance();
}
void GearBox::goHalfSpeed(bool state)
{
	m_isHalfSpeed = state;
}

void GearBox::PIDWrite(float motorValue){
	drive(motorValue);
}

Encoder* GearBox::getEncoderPtr(){
	return mptr_Encoder;
}





